#! /usr/bin/env python

import rospy
import math
import numpy as np
from piksi_rtk_msgs.msg import ReceiverState_V2_4_1
from nav_msgs.msg import Odometry
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

fix_mode = None
odom = Odometry()

def cb_fix_mode(msg):
    global fix_mode
    fix_mode = msg.fix_mode

def cb_odom(msg):
    global odom
    odom = msg

if __name__ == '__main__':

    rospy.init_node('analyze_rtk_quality', anonymous=True)
    rospy.loginfo("Connection Successful!")

    rospy.Subscriber('/odometry/filtered/global', Odometry, cb_odom) #global position topic
    rospy.Subscriber('/piksi_receiver/debug/receiver_state', ReceiverState_V2_4_1, cb_fix_mode)
    rate = rospy.Rate(13)   #set rate to 10Hz

    x_fix = []
    y_fix = []
    x_float = []
    y_float = []

    start_time = rospy.get_time()

    plt.ion()
    plt.title("Estimated position in map frame vs GNNSS fix mode")
    plt.xlabel("x [m] ")
    plt.ylabel("y [m] ")
    plt.show()

    while not rospy.is_shutdown():
        try:
            if fix_mode is not None:
                if fix_mode == "FIXED_RTK":
                    plt.plot(odom.pose.pose.position.x,odom.pose.pose.position.y, 'bx', label="FIXED_RTK")
                    plt.axis("equal")
                    plt.draw()
                    plt.legend(loc="upper right")
                    plt.pause(0.00000000001)
                elif fix_mode == "FLOAT_RTK":
                    plt.plot(odom.pose.pose.position.x,odom.pose.pose.position.y, 'rx', label="FLOAT_RTK")
                    plt.axis("equal")
                    plt.draw()
                    plt.legend(loc="upper right")
                    plt.pause(0.00000000001)
                else:
                    rospy.logwarn("Unsupported fix mode")

            else:
                rospy.loginfo("Waiting for the user to run the bagfile")
        except rospy.ROSInterruptException:
            pass
        rate.sleep()
